#ifndef __GLOBAL_PLANNER_BASE_H__
#define __GLOBAL_PLANNER_BASE_H__

#include <ros/ros.h>
#include <ros/package.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GridCells.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>


#include <iostream>
#include <sstream>
using namespace std;

#include "grid_node.h"
#include "../../core/core.h"
#include "../smoother/path_smoother.h"

// 路径规划类型
enum class GlobalPlannerType{
    A_STAR = 0,
};

// 路径平滑类型
enum class PathSmootherType{
    FIVE_POINTS = 0,
};

// 定义全局路径规划
class GlobalPlannerBase{
    public:
        GlobalPlannerBase(){};
        virtual ~GlobalPlannerBase() = default;
        // 设置调试模式
        virtual void SetDebugMode(bool debug) = 0;
        // 更新缓存数据
        virtual void UpdateCache() = 0;
        // 开始规划路径
        virtual bool GeneratePath(Pose2D start, Pose2D goal, vector<Pose2D>& points, vector<Pixel2D>& expend_points) = 0;

        // 设置地图数据
        void SetMap2D(Map2D map_2d){
            this->map_2d = map_2d;
            UpdateCache();  //更新缓存数据
        }
        
        // 路径平滑
        void PathSmooth(vector<Pose2D> points, vector<Pose2D>& out_points, PathSmootherType type = PathSmootherType::FIVE_POINTS){
            switch(type){
                case PathSmootherType::FIVE_POINTS:
                    PathSmoother::PathSmootherFivePoint(points, &map_2d, out_points);
                    break;
                default:
                    break;
            }  
        }

        Map2D map_2d;     //二维栅格地图数据  
        
    private:

};


#endif
